/*
 * To change this template, choose Tools | Templates
 * and open the template in the editor.
 */

package com.circuitrunners.frc2011.arm;
import com.circuitrunners.frc2011.*;
import edu.wpi.first.wpilibj.*;
import edu.wpi.first.wpilibj.can.CANTimeoutException;
/**
 * The Component Module in charge of controlling the manipulator attached to the second joint of the arm
 * @author Anupam and James
 */
public class manipulator extends Components implements AutoManip {
    /**
     * Allows the program to internally decide on which type of claw are we to have.
     */
    private final int simpleClaw = 1, rollerClaw = 2;
    /**
     * The variable containing which claw we have equipped on the robot at execution time
     */
    private final int manipSwitch = rollerClaw;
    /**
     * Integers containing which state is the claw in at the current moment
     */
    private static final int openState = 0, closeState = 1;
    /**
     * The current state of which Simple claw is in at the moment
     */
    private static int simpleClawState = closeState;
    /**
     * The solenoids for the Simple Claw
     */
    private Solenoid open, close;
    /**
     * The two CAN Jaguars for the roller claw
     */
    private CANJaguar upper, lower;
    /**
     * Initializes the Manipulator component, should only be called once
     */
    public void init(){
        Watchdog.getInstance().feed();
        try {
            upper = new CANJaguar(5); //Intialize both Jags
            lower = new CANJaguar(6);
        } catch (CANTimeoutException ex) {
            ex.printStackTrace();
        }
   }
    /*
     * This method is called by the robot in the teleop period as part of the main iterative loop
     */
    public void run() {
        Watchdog.getInstance().feed();
                try {
                    if(!(getXBOX_BUTTON(5)|| getXBOX_BUTTON(6))){
                        upper.setX(0.5*getXBOX_AXIS(3));
                        lower.setX(0.5*-getXBOX_AXIS(3));
                    }
                    else if(getXBOX_BUTTON(5)){
                        upper.setX(.5);
                        lower.setX(.5);
                    }
                    else if(getXBOX_BUTTON(6)){
                        upper.setX(-.5);
                        lower.setX(-.5);
                    }
                } catch (CANTimeoutException ex) {
                    ex.printStackTrace();
                }
        }
    /*
     * So our robot doesn't go haywire (it happened once before) if we have to disable it.
     */
    public void disable() {
    }
    /**
     * Autonomous method for the Simple Claw
     * @param openClose to open or close the claw (true, open. false, close)
     */
    public void manipulate(boolean openClose) {}
    /**
     * Autonomous method for the Roller Claw
     * @param upperRoller how fast the upper roller is supposed to go (-1.0 to 1.0) 
     * @param lowerRoller how fast the lower roller is supposed to go (-1.0 to 1.0)
     */
    public void manipulate(double upperRoller, double lowerRoller){
        Watchdog.getInstance().feed();
        if(manipSwitch == simpleClaw) return; //Return if simple claww to avoid a NullPointerException
        try{
            upper.setX(upperRoller);
            lower.setX(-lowerRoller);
        } catch(CANTimeoutException ex) {
            ex.printStackTrace();
        }
    }


}
